using System;
using System.Collections.Generic;
using UnityEngine;

namespace Custom.Tools.Curve
{
    public class CatmullRom
    {
        private readonly float MinDistanceBetween2Points = 0.02f;

        public readonly List<CatmullRomSegment> Segments = new();

        private float mTotalDistance = 0;
        private int mLastSegmentCursor = 0;
        private int mLastPosCursor = 0;

        public float TotalDistance { get => mTotalDistance; }

        public Vector3 StartPoint
        {
            get
            {
                if (Segments.Count <= 0)
                {
                    return Vector3.zero;
                }
                return Segments[0].StartNode.point;
            }
        }

        public Vector3 EndPoint
        {
            get
            {
                if (Segments.Count <= 0)
                {
                    return Vector3.zero;
                }
                return Segments[Segments.Count-1].EndNode.point;
            }
        }

        public List<CatmullRomSegment> SplitWayPoints(Vector3[] wayPoints, int amountRadio)
        {
            Segments.Clear();
            mTotalDistance = 0;
            ClearCursor();

            if (wayPoints == null || amountRadio == 0 || wayPoints.Length <= 1)
            {
                return Segments;
            }

            var trailPoint = wayPoints[wayPoints.Length - 1];

            var calcWayPoints = GetWayPoints(wayPoints, amountRadio);
            if (calcWayPoints.Count == 0)
            {
                return Segments;
            }

            var kp = new LinkedList<Vector3>(wayPoints);
            kp.RemoveFirst();

            var wpPos = 0; //计算后的列表，idx
            while (kp.Count > 0)
            {
                var curKeyPoint = kp.First.Value;
                var disMin = float.MaxValue;

                var segment = new CatmullRomSegment();
                var preVec = new Vector3(-1111, -111, -1111);
                for (var i = wpPos; i < calcWayPoints.Count; ++i)
                {
                    var curWpPoint = calcWayPoints[i];

                    var curDis = Vector3.Distance(curKeyPoint, curWpPoint);
                    if (curDis >= disMin)
                    {
                        wpPos = i;
                        break;
                    }
                    else
                    {
                        var tmpDis = Vector3.Distance(curWpPoint, preVec);
                        if (tmpDis > MinDistanceBetween2Points)
                        {
                            //小于最小距离的点，直接丢弃
                            segment.AddNode(curWpPoint);
                            preVec = curWpPoint;
                        }

                        disMin = curDis;
                    }
                }
                segment.index = Segments.Count;
                Segments.Add(segment);

                kp.RemoveFirst();

                if (kp.Count == 0)
                {
                    if (segment.nodes.Count > 0)
                    {
                        var trailNode = segment.nodes[segment.nodes.Count - 1];
                        if (trailNode.point != trailPoint)
                        {
                            segment.AddNode(trailPoint);
                        }
                    }
                }
            }

            CachePositionsAndDistances();

            return Segments;
        }

        public bool GetPoint(float distance, ref RoutePoint routePoint)
        {
            if (GetPoint(distance, out var position, ref routePoint.direction))
            {
                routePoint.position = position;
                return true;
            }
            else {
                routePoint.position = position;
                return false;
            }
        }

        public bool GetPoint(float distance, out Vector3 position, ref Vector3 direction)
        {
            if (distance < 0)
            {
                position = StartPoint;
                return false;
            }

            if (distance > mTotalDistance)
            {
                position = EndPoint;
                return false;
            }

            if (GetRoutePosition(distance, out position, true, true))
            {
                if (GetRoutePosition(distance + 0.01f, out var nextPosition, true, false))
                {
                    direction = (nextPosition - position).normalized;
                }
            }
            return true;
        }

        public bool GetRoutePosition(float distance, out Vector3 position, bool useCursor = true, bool changeCursor = true)
        {
            if (distance <= 0)
            {
                position = StartPoint;
                return false;
            }

            if (distance >= mTotalDistance)
            {
                position = EndPoint;
                return false;
            }

            CatmullRomSegment targetSegment = null;

            int startSegmentIndex = useCursor ? 0 : mLastSegmentCursor;

            //先找到片段
            for (int i = startSegmentIndex; i < Segments.Count; i++)
            {
                //向后查
                var segment = Segments[i];

                if (segment.endDistance >= distance)
                {
                    if (segment.IsInSegment(distance))
                    {
                        targetSegment = segment;
                    }

                    break;
                }
            }

            if (targetSegment == null && startSegmentIndex > 0)
            {
                //如果使用了游标，向后查未找到，则向前查
                for (int i = startSegmentIndex - 1; i >= 0; i--)
                {
                    var segment = Segments[i];

                    if (segment.startDistance <= distance)
                    {
                        if (segment.IsInSegment(distance))
                        {
                            targetSegment = segment;
                        }
                        break;
                    }
                }
            }

            if (targetSegment == null)
            {
                position = EndPoint;
                return false;
            }

            bool isLastSegment = mLastSegmentCursor == targetSegment.index;

            if (changeCursor)
            {
                mLastSegmentCursor = targetSegment.index;
            }

            int posStartIndex = (isLastSegment && useCursor) ? mLastPosCursor : 0;
            CatmullRomSegmentNode targetNode = null;
            for (int i = posStartIndex; i < targetSegment.nodes.Count; i++)
            {
                //向后查
                var node = targetSegment.nodes[i];
                if (node.endDistance >= distance)
                {
                    if (node.IsTargetNode(distance))
                    {
                        targetNode = node;
                    }
                    break;
                }
            }

            if (targetNode == null && posStartIndex > 0)
            {
                //向前查
                for (int i = posStartIndex - 1; i >= 0; i--)
                {
                    var node = targetSegment.nodes[i];
                    if (node.preDistance <= distance)
                    {
                        if (node.IsTargetNode(distance))
                        {
                            targetNode = node;
                        }
                        break;
                    }
                }
            }

            if (targetNode == null)
            {
                position = EndPoint;
                return false;
            }

            if ((targetSegment.index == 0 && targetNode.index == 0) || targetNode.endDistance == distance || targetNode.durationDistance == 0)
            {
                position = targetNode.point;
            }
            else
            {
                CatmullRomSegmentNode preNode = null;
                if (targetNode.index == 0)
                {
                    //找到上一个片段
                    var lastSegment = Segments[targetSegment.index - 1];
                    preNode = lastSegment.EndNode;
                }
                else
                {
                    preNode = targetSegment.nodes[targetNode.index - 1];
                }
                float rate = (distance - targetNode.preDistance) / (targetNode.durationDistance);
                position = (targetNode.point - preNode.point) * rate + preNode.point;
            }

            return true;
        }

        private void CachePositionsAndDistances()
        {
            mTotalDistance = 0;
            float accumulateDistance = 0;

            var prePoint = Vector3.zero;
            for (int i = 0; i < Segments.Count; i++)
            {
                var segment = Segments[i];
                var ns = segment.nodes;
                segment.startDistance = accumulateDistance; //每段的长度
                for (int j = 0; j < ns.Count; j++)
                {
                    var node = ns[j];
                    node.preDistance = accumulateDistance;
                    if (i == 0 && j == 0)
                    {
                        accumulateDistance = 0;
                        node.durationDistance = 0;
                    }
                    else {
                        node.durationDistance = (node.point - prePoint).magnitude;
                        accumulateDistance += node.durationDistance;
                    }

                    node.endDistance = accumulateDistance;
                    prePoint = node.point;
                }
                segment.endDistance = accumulateDistance; //每段的长度
            }

            mTotalDistance = accumulateDistance;
        }

        private void ClearCursor()
        {
            mLastSegmentCursor = 0;
            mLastPosCursor = 0;
        }

        private static List<Vector3> GetWayPoints(Vector3[] points, int amountRadio)
        {
            var SmoothAmount = (points.Length - 1) * amountRadio;
            var wayPoints = new List<Vector3>(SmoothAmount+1);
            if (points == null || points.Length <= 1)
            {
                Debug.Log("points is empty!");
                return new List<Vector3>();
            }

            var vector3s = PathControlPointGenerator(points);
            wayPoints.Add(Interp(vector3s, 0));
            for (var i = 1; i <= SmoothAmount; i++)
            {
                wayPoints.Add(Interp(vector3s, (float)i / SmoothAmount));
            }

            return wayPoints;
        }

        private static Vector3[] PathControlPointGenerator(Vector3[] path)
        {
            var suppliedPath = path;

            var vector3s = new Vector3[suppliedPath.Length + 2];
            Array.Copy(suppliedPath, 0, vector3s, 1, suppliedPath.Length);

            vector3s[0] = vector3s[1];
            vector3s[vector3s.Length - 1] = vector3s[vector3s.Length - 2];

            return vector3s;
        }

        private static Vector3 Interp(Vector3[] pts, float t)
        {
            var numSections = pts.Length - 3;
            var currPt = numSections - 1;
            if (currPt > t * numSections)
            {
                currPt = Mathf.FloorToInt(t * numSections);
            }

            var u = t * numSections - currPt;
            var u2 = u * u;
            var u3 = u2 * u;

            var a = pts[currPt];
            var b = pts[currPt + 1];
            var c = pts[currPt + 2];
            var d = pts[currPt + 3];

            return .5f * ((-a + 3f * b - 3f * c + d) * u3 + (2f * a - 5f * b + 4f * c - d) * u2 + (-a + c) * u + 2f * b);
        }
    }

    public struct RoutePoint
    {
        public Vector3 position;
        public Vector3 direction;


        public RoutePoint(Vector3 position, Vector3 direction)
        {
            this.position = position;
            this.direction = direction;
        }
    }

    public class CatmullRomSegmentNode
    {
        public int index;
        public Vector3 point;
        public float preDistance;
        public float endDistance;
        public float durationDistance;

        public bool IsTargetNode(float distance)
        {
            return distance >= preDistance && distance <= endDistance;
        }
    }

    public class CatmullRomSegment
    {
        //public List<Vector3> points = new();
        public List<float> distances = new();

        public List<CatmullRomSegmentNode> nodes = new();

        public float startDistance = 0;
        public float endDistance = 0;

        public int index;

        public void AddNode(Vector3 pos)
        {
            var node = new CatmullRomSegmentNode();
            node.point = pos;
            node.index = nodes.Count;

            nodes.Add(node);
        }

        public CatmullRomSegmentNode StartNode
        {
            get
            {
                if (nodes.Count <= 0)
                {
                    return new();
                }
                return nodes[0];
            }
        }

        public CatmullRomSegmentNode EndNode
        {
            get
            {
                if (nodes.Count <= 0)
                {
                    return new();
                }
                return nodes[nodes.Count - 1];
            }
        }

        public bool IsInSegment(float distance)
        {
            return distance >= startDistance && distance <= endDistance;
        }
    }


}